
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       fms_commands.c
  * @author     baiyang
  * @date       2023-7-6
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "fms.h"

#include <uITC/uITC.h>
#include <uITC/uITC_msg.h>

#if CONFIG_HAL_BOARD != HAL_BOARD_SITL_WIN
#include <sensor_baro/sensor_baro.h>
#endif

#include <notify/notify.h>
#include <ahrs/ahrs_view.h>
#include <mission/mb_mission.h>
#include <common/time/gp_time.h>
#include <common/geo/declination.h>
#include <board_config/board_config.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/*
  update home location from GPS
  this is called as long as we have 3D lock and the arming switch is
  not pushed

  returns true if home is changed
*/
bool fms_update_home()
{
    uitc_sensor_gps vehicle_gps_position;
    itc_copy_from_hub(ITC_ID(sensor_gps),  &vehicle_gps_position);

    if ((fms.g.home_reset_threshold == -1) ||
        ((fms.g.home_reset_threshold > 0) &&
         (fabsf(ahrs_get_relative_position_u_home(fms.ahrs)) > fms.g.home_reset_threshold))) {
        // don't auto-update if we have changed barometer altitude
        // significantly. This allows us to cope with slow baro drift
        // but not re-do home and the baro if we have changed height
        // significantly
        return false;
    }
    bool ret = false;
    if (ahrs_home_is_set(fms.ahrs) && !ahrs_home_is_locked(fms.ahrs) && vehicle_gps_position.fix_type >= uITC_GPS_OK_FIX_3D) {
        Location loc;
        if (ahrs_get_location(fms.ahrs, &loc)) {
            // we take the altitude directly from the GPS as we are
            // about to reset the baro calibration. We can't use AHRS
            // altitude or we can end up perpetuating a bias in
            // altitude, as AHRS alt depends on home alt, which means
            // we would have a circular dependency
            loc.alt = vehicle_gps_position.alt_msl;
            ret = ahrs_set_home(fms.ahrs, &loc);
        }
    }

#if CONFIG_HAL_BOARD != HAL_BOARD_SITL_WIN
    // even if home is not updated we do a baro reset to stop baro
    // drift errors while disarmed
    sensor_baro_update_calibration();
    //ahrs_resetHeightDatum();
#endif

    fms_update_current_loc();

    return ret;
}

bool fms_set_home_persistently(const Location *loc)
{
    if (!ahrs_set_home(fms.ahrs, loc)) {
        return false;
    }

    // Save Home to EEPROM
    mission_write_home_to_storage();

    return true;
}

/*------------------------------------test------------------------------------*/


